#include "spider_leg_kinematics.h"
#include <stdio.h>

/************************** 辅助工具（参考arm.cpp） **************************/
#define DEG_TO_RAD(deg)  ((deg) * PI / 180.0f)
#define RAD_TO_DEG(rad)  ((rad) * 180.0f / PI)
#ifndef PI
#define PI 3.14159265358979323846f
#endif

// 关节角度限制（参考arm.cpp注释的value_limit函数）
static void Leg_LimitAngle(float* angle) {
    if (*angle < SERVO_MIN) *angle = SERVO_MIN;
    else if (*angle > SERVO_MAX) *angle = SERVO_MAX;
}

// 检查NaN值（参考arm.cpp ikine函数）
static uint8_t Leg_IsNaN(float val) {
    return (val != val) ? 1 : 0;
}

/************************** 正解 **************************/
LegStatus Leg_ForwardKinematics(ServoAngles servo_ang, D4Coord* d4_coord) {
    // 参数校验（参考arm.cpp set_pos逻辑）
    if (d4_coord == NULL) {
        //printf("[正解] 错误：D4坐标指针为空\n");
        return LEG_ERR_PARAM;
    }

    // 1. 用户舵机角度→《main.pdf》θ₁/θ₂/θ₃（关键映射）
    float theta1 = DEG_TO_RAD(servo_ang.body);                  // 底座关节（θ₁）
    float theta2 = DEG_TO_RAD(servo_ang.limb);                  // 大臂关节（θ₂）
    float theta3 = DEG_TO_RAD(USER_FOOT_TO_DOC_THETA3(servo_ang.foot));  // 小臂关节（θ₃）

    // 2. 《main.pdf》2.1.1节正解公式（式2-3）
    float cos_theta1 = cosf(theta1);
    float sin_theta1 = sinf(theta1);
    float cos_theta2 = cosf(theta2);
    float sin_theta2 = sinf(theta2);
    float theta2_3 = theta2 + theta3;  // θ₂+θ₃（合并项）
    float cos_theta23 = cosf(theta2_3);
    float sin_theta23 = sinf(theta2_3);

    // 水平段总长：L₁ + L₂cosθ₂ + L₃cos(θ₂+θ₃)
    float L_total = LEG_L1 + LEG_L2 * cos_theta2 + LEG_L3 * cos_theta23;

    // 计算D4坐标（严格匹配公式，z向下为负）
    d4_coord->x = L_total * cos_theta1;
    d4_coord->y = L_total * sin_theta1;
    d4_coord->z = LEG_L2 * sin_theta2 + LEG_L3 * sin_theta23;  // 式2-3直接使用，θ₃=-90°时z为负

    // 输出结果（用户可直接查看）
    //printf("\n[正解] 输入舵机角度：body=%.1f°, limb=%.1f°, foot=%.1f°\n",
    //       servo_ang.body, servo_ang.limb, servo_ang.foot);
    //printf("[正解] 输出D4坐标：x=%.2fmm, y=%.2fmm, z=%.2fmm\n",
    //       d4_coord->x, d4_coord->y, d4_coord->z);

    return LEG_OK;
}

/************************** 逆解几何法 **************************/
LegStatus Leg_InverseKinematics(D4Coord target_coord, ServoAngles* servo_ang) {
    // 参数校验（参考arm.cpp ikine逻辑）
    if (servo_ang == NULL) {
        //printf("[逆解] 错误：舵机角度指针为空\n");
        return LEG_ERR_PARAM;
    }

    // 1. 步骤1：计算底座关节角θ₁（《main.pdf》式2-6）
    float R = sqrtf(target_coord.x * target_coord.x + target_coord.y * target_coord.y);  // XY投影距离
    float theta1 = (R < 0.001f) ? 0.0f : atan2f(target_coord.y, target_coord.x);  // θ₁=atan2(y,x)
    servo_ang->body = RAD_TO_DEG(theta1);
    Leg_LimitAngle(&servo_ang->body);  // 限制舵机范围

    // 2. 步骤2：计算辅助参数（《main.pdf》图2-3）
    float R_minus_L1 = R - LEG_L1;                // R-L₁（关节1→关节2的水平偏移）
    float Lr = sqrtf(target_coord.z * target_coord.z + R_minus_L1 * R_minus_L1);  // 关节2→脚尖距离Lr

    // 3. 步骤3：工作空间检查（《main.pdf》2.1.2节三角不等式）
    if (Lr < fabsf(LEG_L2 - LEG_L3) || Lr > (LEG_L2 + LEG_L3)) {
       // printf("[逆解] 错误：超出工作空间（Lr=%.2fmm，需%.2f~%.2fmm）\n",
               //Lr, fabsf(LEG_L2-LEG_L3), LEG_L2+LEG_L3);
        return LEG_ERR_WORKSPACE;
    }

    // 4. 步骤4：计算大臂关节角θ₂（《main.pdf》α₁-α_R）
    float cos_alpha1 = (LEG_L2*LEG_L2 + Lr*Lr - LEG_L3*LEG_L3) / (2 * LEG_L2 * Lr);  // 余弦定理
    cos_alpha1 = (cos_alpha1 < -1.0f) ? -1.0f : (cos_alpha1 > 1.0f) ? 1.0f : cos_alpha1;
    float alpha1 = acosf(cos_alpha1);  // α₁（关节2角度分量）
    float alpha_R = atan2f(-target_coord.z, R_minus_L1);  // α_R=atan(-z/(R-L₁))（《main.pdf》公式）
    float theta2 = alpha1 - alpha_R;  // θ₂=α₁-α_R
    servo_ang->limb = RAD_TO_DEG(theta2);
    Leg_LimitAngle(&servo_ang->limb);  // 限制舵机范围

    // 5. 步骤5：计算小臂关节角θ₃（《main.pdf》-(α₁+α₂)）
    float cos_alpha2 = (Lr*Lr + LEG_L3*LEG_L3 - LEG_L2*LEG_L2) / (2 * Lr * LEG_L3);  // 余弦定理
    cos_alpha2 = (cos_alpha2 < -1.0f) ? -1.0f : (cos_alpha2 > 1.0f) ? 1.0f : cos_alpha2;
    float alpha2 = acosf(cos_alpha2);  // α₂（关节3角度分量）
    float theta3 = -(alpha1 + alpha2);  // θ₃=-(α₁+α₂)（《main.pdf》式2-8）
    // 映射到用户舵机基准：foot=θ₃+90°
    servo_ang->foot = DOC_THETA3_TO_USER_FOOT(RAD_TO_DEG(theta3));
    Leg_LimitAngle(&servo_ang->foot);  // 限制舵机范围

    // 6. 步骤6：检查解算有效性（参考arm.cpp NaN判断）
    if (Leg_IsNaN(servo_ang->body) || Leg_IsNaN(servo_ang->limb) || Leg_IsNaN(servo_ang->foot)) {
       // printf("[逆解] 错误：解算结果含NaN（位置不可达）\n");
        return LEG_ERR_WORKSPACE;
    }

    // 输出结果（用户可直接驱动舵机）
    //printf("\n[逆解] 输入D4坐标：x=%.2fmm, y=%.2fmm, z=%.2fmm\n",
    //       target_coord.x, target_coord.y, target_coord.z);
    //printf("[逆解] 输出舵机角度：body=%.1f°, limb=%.1f°, foot=%.1f°\n",
    //       servo_ang->body, servo_ang->limb, servo_ang->foot);

    return LEG_OK;
}

/************************** 步态组解算（二步态） **************************/
LegStatus Leg_GaitGroupSolve(GaitGroup group, D4Coord target_coords[3], ServoAngles servo_angs[3]) {
    // 二步态分组：GroupA（腿1/3/5）、GroupB（腿2/4/6）
    const char* group_name = (group == GAIT_GROUP_A) ? "A（腿1/3/5）" : "B（腿2/4/6）";
    //printf("\n=== 步态组解算：%s ===\n", group_name);

    // 批量解算组内3条腿
    for (uint8_t i = 0; i < 3; i++) {
        //printf("\n[组解算] 第%d条腿：\n", (group==GAIT_GROUP_A) ? (i*2+1) : (i*2+2));
        if (Leg_InverseKinematics(target_coords[i], &servo_angs[i]) != LEG_OK) {
            //printf("[组解算] 第%d条腿解算失败\n", (group==GAIT_GROUP_A) ? (i*2+1) : (i*2+2));
            return LEG_ERR_WORKSPACE;
        }
    }

    //printf("=== 步态组%s解算完成 ===\n", group_name);
    return LEG_OK;
}

/************************** 闭环验证 **************************/
uint8_t Leg_CloseLoopVerify(ServoAngles input_ang, float max_err) {
    //printf("\n=== 正逆解闭环验证 ===\n");
    D4Coord mid_coord;
    ServoAngles output_ang;

    // 1. 正解：输入角度→坐标
    if (Leg_ForwardKinematics(input_ang, &mid_coord) != LEG_OK) {
        //printf("[验证] 正解失败\n");
        return 0;
    }

    // 2. 逆解：坐标→输出角度
    if (Leg_InverseKinematics(mid_coord, &output_ang) != LEG_OK) {
        //printf("[验证] 逆解失败\n");
        return 0;
    }

    // 3. 计算误差（用户舵机角度偏差）
    float err_body = fabsf(input_ang.body - output_ang.body);
    float err_limb = fabsf(input_ang.limb - output_ang.limb);
    float err_foot = fabsf(input_ang.foot - output_ang.foot);

    // 4. 验证结果（参考《main.pdf》6章实验标准）
    // printf("[验证] 输入角度：body=%.1f°, limb=%.1f°, foot=%.1f°\n",
    //        input_ang.body, input_ang.limb, input_ang.foot);
    // printf("[验证] 输出角度：body=%.1f°, limb=%.1f°, foot=%.1f°\n",
    //        output_ang.body, output_ang.limb, output_ang.foot);
    // printf("[验证] 误差：body=%.2f°, limb=%.2f°, foot=%.2f°\n",
    //        err_body, err_limb, err_foot);

    if (err_body <= max_err && err_limb <= max_err && err_foot <= max_err) {
        //printf("[验证] 成功：误差≤%.2f°（符合《main.pdf》实验标准）\n", max_err);
        return 1;
    } else {
        //printf("[验证] 失败：误差超限\n");
        return 0;
    }
}


// 初始化6条腿的安装参数（对齐《main.pdf》附录MATLAB代码Leg.base规则）
// 文档规则：X正=向前，Y正=向左；左侧腿Y正/角度正，右侧腿Y负/角度负，前后腿X正负区分
//这个是6条腿的腿部第一个关节坐标及旋转参数
LegMountParam leg_mount_params[6] = {
    // 腿1：前方左侧（对应文档Leg1，X前+、Y左+，角度正）
    {.offset_x = 80.0f, .offset_y = 50.0f, .offset_z = 0.0f, .mount_angle = 50.0f},
    // 腿2：中间左侧（对应文档Leg3，X中0、Y左+，角度正）
    {.offset_x = 0.0f, .offset_y = 60.0f, .offset_z = 0.0f, .mount_angle = 90.0f},
    // 腿3：后方左侧（对应文档Leg5，X后-、Y左+，角度正）
    {.offset_x = -80.0f, .offset_y = 50.0f, .offset_z = 0.0f, .mount_angle = 130.0f},
    // 腿4：后方右侧（对应文档Leg6，X后-、Y右-，角度负）
    {.offset_x = -80.0f, .offset_y = -50.0f, .offset_z = 0.0f, .mount_angle = -130.0f},
    // 腿5：中间右侧（对应文档Leg4，X中0、Y右-，角度负）
    {.offset_x = 0.0f, .offset_y = -60.0f, .offset_z = 0.0f, .mount_angle = -90.0f},
    // 腿6：前方右侧（对应文档Leg2，X前+、Y右-，角度负）
    {.offset_x = 80.0f, .offset_y = -50.0f, .offset_z = 0.0f, .mount_angle = -50.0f},
};

// 局部坐标→全局坐标（先旋转，再平移）
void Leg_LocalToGlobal(uint8_t leg_idx, D4Coord local_coord, D4Coord* global_coord) {
    if (leg_idx >= 6 || global_coord == NULL) return;
    LegMountParam* param = &leg_mount_params[leg_idx];
    float theta = DEG_TO_RAD(param->mount_angle);  // 转为弧度
    float cos_theta = cosf(theta);
    float sin_theta = sinf(theta);

    // 绕Z轴旋转局部坐标（对齐全局朝向）
    float x_rot = local_coord.x * cos_theta - local_coord.y * sin_theta;
    float y_rot = local_coord.x * sin_theta + local_coord.y * cos_theta;
    float z_rot = local_coord.z;

    // 加上底座关节的全局偏移
    global_coord->x = x_rot + param->offset_x;
    global_coord->y = y_rot + param->offset_y;
    global_coord->z = z_rot + param->offset_z;
}

// 全局坐标→局部坐标（先平移，再逆旋转）
void Leg_GlobalToLocal(uint8_t leg_idx, D4Coord global_coord, D4Coord* local_coord) {
    if (leg_idx >= 6 || local_coord == NULL) return;
    LegMountParam* param = &leg_mount_params[leg_idx];
    float theta = DEG_TO_RAD(param->mount_angle);  // 转为弧度
    float cos_theta = cosf(theta);
    float sin_theta = sinf(theta);

    // 减去底座关节的全局偏移
    float x_trans = global_coord.x - param->offset_x;
    float y_trans = global_coord.y - param->offset_y;
    float z_trans = global_coord.z - param->offset_z;

    // 绕Z轴逆旋转（恢复局部朝向）
    local_coord->x = x_trans * cos_theta + y_trans * sin_theta;  // 逆旋转矩阵
    local_coord->y = -x_trans * sin_theta + y_trans * cos_theta;
    local_coord->z = z_trans;
}


// 全局正解：舵机角度→全局坐标（调用原有正解+坐标转换）
LegStatus Leg_ForwardKinematicsGlobal(uint8_t leg_idx, ServoAngles servo_ang, D4Coord* global_coord) {
    if (leg_idx >= 6 || global_coord == NULL) {
        //printf("[全局正解] 错误：腿索引无效或坐标指针为空\n");
        return LEG_ERR_PARAM;
    }
    D4Coord local_coord;
    LegStatus status = Leg_ForwardKinematics(servo_ang, &local_coord);  // 复用原有局部正解
    if (status == LEG_OK) {
        Leg_LocalToGlobal(leg_idx, local_coord, global_coord);  // 转换为全局坐标
        //printf("[全局正解] 腿%d 全局坐标：x=%.2f, y=%.2f, z=%.2f mm\n",
        //       leg_idx+1, global_coord->x, global_coord->y, global_coord->z);
    }
    return status;
}

// 全局逆解：全局坐标→舵机角度（坐标转换+调用原有逆解）
LegStatus Leg_InverseKinematicsGlobal(uint8_t leg_idx, D4Coord global_coord, ServoAngles* servo_ang) {
    if (leg_idx >= 6 || servo_ang == NULL) {
        //printf("[全局逆解] 错误：腿索引无效或角度指针为空\n");
        return LEG_ERR_PARAM;
    }
    D4Coord local_coord;
    Leg_GlobalToLocal(leg_idx, global_coord, &local_coord);  // 转换为局部坐标
    LegStatus status = Leg_InverseKinematics(local_coord, servo_ang);  // 复用原有局部逆解
    if (status == LEG_OK) {
        //printf("[全局逆解] 腿%d 舵机角度解算完成\n", leg_idx+1);
    }
    return status;
}

// 全局步态组解算（支持二步态分组的全局坐标输入）
LegStatus Leg_GaitGroupSolveGlobal(GaitGroup group, D4Coord global_coords[3], ServoAngles servo_angs[3]) {
    const char* group_name = (group == GAIT_GROUP_A) ? "A（腿1/3/5）" : "B（腿2/4/6）";
    //printf("\n=== 全局步态组解算：%s ===\n", group_name);

    // 组内腿索引（0~5对应腿1~6）
    uint8_t leg_indices[3];

    if (group == GAIT_GROUP_A) {
        leg_indices[0] = 0;  // 腿1
        leg_indices[1] = 2;  // 腿3
        leg_indices[2] = 4;  // 腿5
    } else {
        leg_indices[0] = 1;  // 腿2
        leg_indices[1] = 3;  // 腿4
        leg_indices[2] = 5;  // 腿6
    }

    for (uint8_t i = 0; i < 3; i++) {
        //printf("\n[全局组解算] 第%d条腿：\n", leg_indices[i]+1);
        if (Leg_InverseKinematicsGlobal(leg_indices[i], global_coords[i], &servo_angs[i]) != LEG_OK) {
        //    printf("[全局组解算] 第%d条腿解算失败\n", leg_indices[i]+1);
            return LEG_ERR_WORKSPACE;
        }
    }
    //printf("=== 全局步态组%s解算完成 ===\n", group_name);
    return LEG_OK;
}


